// --------------------------------------------------------------------------------------------------------------------
// <copyright file="NavigationMetricsAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using MicrosoftResearch.Infer.Distributions;
    using Test.Robotics.Metrics;

    /// <summary>
    /// Agent that compares the robot positioning estimate from the navigation pipeline with an external ground truth source.
    /// </summary>
    [DataContract]
    public class NavigationMetricsAgent : SynchronizedConsumerProducerAgent<NavDataMessage, RobotPoseEstimationAgentMessage, MetricsResultMessage>
    {
        /// <summary>
        /// The maximum discrepancy between navigation and pose originating times.
        /// Images are captured every 1 seconds and the robot is instructed to wait 10 seconds at each waypoint.
        /// </summary>
        private const int DefaultSyncTolerance = 1100;

        /// <summary>
        /// The name of the agent producing the navigation metrics messages
        /// </summary>
        [DataMember(Name = "NavDataProducer")]
        private readonly string navDataProducerName;

        /// <summary>
        /// The name of the robot pose estimation agent
        /// </summary>
        [DataMember(Name = "RobotPoseProducer")]
        private readonly string robotPoseProducerName;

        /// <summary>
        /// The name of the agent we use to update the landmark index.
        /// </summary>
        [DataMember(Name = "CameraPoseProducer")]
        private readonly string cameraPoseProducerName;

        /// <summary>
        /// Path to the file containing a serialized matrix of distances
        /// </summary>
        [DataMember(Name = "DistancesFilePath")]
        private string distancesFilePath;

        /// <summary>
        /// Path to the file containing the ground truth locations for the currently used SLAM map
        /// </summary>
        [DataMember(Name = "GroundTruthFilePath")]
        private string groundTruthFilePath;

        /// <summary>
        /// Should special database messages be added
        /// </summary>
        [DataMember(Name = "Database")]
        private bool database = false;

        /// <summary>
        /// Camera Pose Estimation Input Forwarder used to pass the UI updates to the camera pose estimation agent.
        /// </summary>
        private IForwarder<CameraPoseEstimationInputMessage> cameraPoseEstimationInputForwarder;

        /// <summary>
        /// The id of the iteration that generated this message. Used to tie all messages from the same run together.
        /// </summary>
        private long runId;

        /// <summary>
        /// Worker class for navigation metrics
        /// </summary>
        private NavigationMetrics navMetrics;

        /// <summary>
        /// The previous landmark ID.  Used for looking up distances.
        /// </summary>
        private int lastLandmarkId;

        /// <summary>
        /// Track when a missing pose causes the run to be terminated
        /// </summary>
        private bool runTerminatedDueToNullPose;

        /// <summary>
        /// Track if database start messages have been sent
        /// </summary>
        private bool startMessagesSent;

        /// <summary>
        /// Initializes a new instance of the <see cref="NavigationMetricsAgent" /> class.
        /// </summary>
        /// <param name="name">The name of the calculator agent</param>
        /// <param name="navDataProducer">The name of the slam agent producing the measured data</param>
        /// <param name="robotPoseProducer">The name of the ground truth agent we compare with</param>
        /// <param name="cameraPoseProducer">The name of the camera pose producer agent used to update landmark ids</param>
        /// <param name="distancesFilePath">The path to the serialize matrix of distances</param>
        /// <param name="groundTruthFilePath">The path to the absolut ground truth </param>
        /// <param name="database">Indicates if results are going to a database</param>
        public NavigationMetricsAgent(string name, string navDataProducer, string robotPoseProducer, string cameraPoseProducer, string distancesFilePath, string groundTruthFilePath, bool database = false)
            : base(name, DefaultSyncTolerance, true)
        {
            this.navDataProducerName = navDataProducer;
            this.robotPoseProducerName = robotPoseProducer;
            this.cameraPoseProducerName = cameraPoseProducer;
            this.distancesFilePath = distancesFilePath;
            this.groundTruthFilePath = groundTruthFilePath;
            this.database = database;
        }

        /// <summary>
        /// The name of the slam agent we measure
        /// </summary>
        protected override string ProducerName1
        {
            get { return this.navDataProducerName; }
        }

        /// <summary>
        /// The name of the ground truth agent we compare with
        /// </summary>
        protected override string ProducerName2
        {
            get { return this.robotPoseProducerName; }
        }

        /// <summary>
        /// Initialize the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.cameraPoseEstimationInputForwarder = locator.GetAgent(this.cameraPoseProducerName).GetControlPort<CameraPoseEstimationInputMessage>();
        }

        /// <summary>
        /// Initializes the agent on activation. This acts as a constructor because the agent is activated/deactivated multiple times.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.runId = AgentMessage.GetCurrentTime();
            this.navMetrics = new NavigationMetrics(this.distancesFilePath, this.groundTruthFilePath, this.runId);
        }

        /// <summary>
        /// Called when a pair of messages becomes available
        /// </summary>
        /// <param name="navDataMessage">The message containing the measured data</param>
        /// <param name="robotPoseMessage">The ground truth message we compare with</param>
        protected override void Receive(NavDataMessage navDataMessage, RobotPoseEstimationAgentMessage robotPoseMessage)
        {
            if (navDataMessage.LandmarkId != robotPoseMessage.Landmark.Id)
            {
                throw new InvalidOperationException("Landmark ID from the pose estimator does not match the nav data message");
            }
            
            if (this.runTerminatedDueToNullPose)
            {
                Console.WriteLine("Run was terminated (probably due to a missing [null] pose), but an additional nav data message was received.");
                Console.WriteLine("This is an unexpected condition and should be investigated.\n");
                Console.WriteLine("A likely explanation is that the robot was at a previous goal, but the ground truth extraction system failed to read a pose from the synchronized image and the run was not terminated.");
            }

            if (robotPoseMessage.RobotPose == null)
            {
                // This means a pose could not be generated from the image at the time the robot declared it was waiting at a goal.
                // Our only recourse at this moment is to assume the robot was lost and end the run.
                // The run will be a failure in the database by not posting the final "success" messages.
                // If there are future navDataMessages, they should be ignored for this run (since we have no idea where the robot started)
                this.runTerminatedDueToNullPose = true;
                Console.WriteLine("Null pose received.  Run terminated.");
            }

            if (this.database && !this.startMessagesSent)
            {
                this.PostDatabaseFlagMessages(-1);
                this.startMessagesSent = true;
            }

            this.Publisher.Post(this.navMetrics.GetLocPositionResult(navDataMessage, robotPoseMessage, this.lastLandmarkId));
            this.Publisher.Post(this.navMetrics.GetLocOrientationResult(navDataMessage, robotPoseMessage));
            this.Publisher.Post(this.navMetrics.GetNavPositionResult(navDataMessage, robotPoseMessage, this.lastLandmarkId));
            this.Publisher.Post(this.navMetrics.GetNavOrientationResult(navDataMessage, robotPoseMessage));
            
            // There is one case when the start and end can be the same, and that is when the first random destination is also where the robot started.
            // In this case calculating speed does not make sense.
            if (this.lastLandmarkId != navDataMessage.LandmarkId)
            {
                this.Publisher.Post(this.navMetrics.GetNavSpeedResult(navDataMessage, this.lastLandmarkId));
            }

            this.lastLandmarkId = navDataMessage.LandmarkId;

            if (navDataMessage.NextLandmarkId == -2)
            {
                // if the next landmark is -2 that means the robot visited all the intended goals for this run
                Console.WriteLine("All landmarks reached");
                if (this.database)
                {
                    this.PostDatabaseFlagMessages(-2);
                }
            }
            else
            {
                this.cameraPoseEstimationInputForwarder.Post(new CameraPoseEstimationInputMessage(navDataMessage.NextLandmarkId));
            }
        }

        /// <summary>
        /// Posts the special flag messages to the database
        /// </summary>
        /// <param name="flag">Value to us as the flag (will become the landmarkId in the database)</param>
        /// <remarks>
        /// -1 is a magic number in the database that means the "start" of a run
        /// -2 is a magic number in the database that means the "end" of a run
        /// </remarks>
        private void PostDatabaseFlagMessages(int flag)
        {
            this.PostDatabaseRunMessage("NavPosition", flag);
            this.PostDatabaseRunMessage("NavOrientation", flag);
            this.PostDatabaseRunMessage("NavSpeed", flag);
        }

        /// <summary>
        /// Posts a special message with specified flag as the ground truth ID.
        /// </summary>
        /// <param name="type">The type of message</param>
        /// <param name="flag">Value to pass as the ground truth ID</param>
        private void PostDatabaseRunMessage(string type, int flag)
        {
            this.Publisher.Post(
                new MetricsResultMessage(
                type: type,
                groundTruthId: flag,
                accuracy: 0,
                precision: 0,
                weight: 0,
                debugData: string.Empty,
                duration: 0,
                robot: null,
                runId: this.runId,
                originatingMessage: new AgentMessage(AgentMessage.GetCurrentTime())),
                true);
        }
    }
}
